#include <stdio.h>
#include "Nano100Series.h"
#include "Bmx055.h"


#define MIN(x,y) (x < y ? x : y)

#define BMX055_CHANNEL_ACC	1
#define BMX055_CHANNEL_GYRO	0
#define BMX055_CHANNEL_MAG	2

static void Bmx055Read(uint8_t ch, uint8_t reg_addr, uint8_t *reg_value, uint32_t n)
{
	int i;

	if (ch == 0) {
		SPI_SET_SS0_LOW(SPI1);
		SPI_SET_SS1_HIGH(SPI1);
		BMX055_CS2_PIN = 1;
	} else if (ch == 1) {
		SPI_SET_SS0_HIGH(SPI1);
		SPI_SET_SS1_LOW(SPI1);
		BMX055_CS2_PIN = 1;
	} else {
		SPI_SET_SS0_HIGH(SPI1);
		SPI_SET_SS1_HIGH(SPI1);
		BMX055_CS2_PIN = 0;
	}

	SPI_WRITE_TX0(SPI1, reg_addr | 0X80);
	SPI_TRIGGER(SPI1);
	while(SPI_IS_BUSY(SPI1));

	for (i = 0; i < n; i++) {
		SPI_WRITE_TX0(SPI1, 0);
		SPI_TRIGGER(SPI1);
		while(SPI_IS_BUSY(SPI1));
		reg_value[i] = SPI_READ_RX0(SPI1);
	}

	SPI_SET_SS0_HIGH(SPI1);
	SPI_SET_SS1_HIGH(SPI1);
	BMX055_CS2_PIN = 1;
}

static Bmx055Write(uint8_t ch, uint8_t reg_addr, uint8_t *reg_value, uint32_t n)
{
	int i;

	if (ch == 0) {
		SPI_SET_SS0_LOW(SPI1);
		SPI_SET_SS1_HIGH(SPI1);
		BMX055_CS2_PIN = 1;
	} else if (ch == 1) {
		SPI_SET_SS0_HIGH(SPI1);
		SPI_SET_SS1_LOW(SPI1);
		BMX055_CS2_PIN = 1;
	} else {
		SPI_SET_SS0_HIGH(SPI1);
		SPI_SET_SS1_HIGH(SPI1);
		BMX055_CS2_PIN = 0;
	}

	SPI_WRITE_TX0(SPI1, reg_addr);
	SPI_TRIGGER(SPI1);
	while(SPI_IS_BUSY(SPI1));

	for (i = 0; i < n; i++) {
		SPI_WRITE_TX0(SPI1, reg_value[i]);
		SPI_TRIGGER(SPI1);
		while(SPI_IS_BUSY(SPI1));
	}

	SPI_SET_SS0_HIGH(SPI1);
	SPI_SET_SS1_HIGH(SPI1);
	BMX055_CS2_PIN = 1;
}

static void Bmx055Write8(uint8_t ch, uint8_t reg_addr, uint8_t reg_value)
{
	Bmx055Write(ch, reg_addr, &reg_value, 1);
}

void Bmx055Init()
{
	SPI_Open(SPI1, SPI_MASTER, SPI_MODE_0, 8, 2000000);
	SPI_DisableAutoSS(SPI1);
	BMX055_CS2_PIN = 1;

	DelayMs(1);

	/* accelerometer */
	Bmx055Write8(BMX055_CHANNEL_ACC, 0x14, 0xB6);
	DelayMs(1);

	Bmx055Write8(BMX055_CHANNEL_ACC, 0x10, 0x0C);	// BW : 1000 Hz(0x0F) 125 Hz(0x0C)
	DelayMs(1);

	Bmx055Write8(BMX055_CHANNEL_ACC, 0x0F, 0x03);	// RANGE : 2G
	DelayMs(1);

	/* gyroscope */
	Bmx055Write8(BMX055_CHANNEL_GYRO, 0x14, 0xB6);
	DelayMs(1);

	Bmx055Write8(BMX055_CHANNEL_GYRO, 0x10, 0x05);	// ODR : 1000 Hz(0x02) 100 Hz(0x05)
	DelayMs(1);

	Bmx055Write8(BMX055_CHANNEL_GYRO, 0x0F, 0x02);	// RANGE 500 d/s
	DelayMs(1);

	/* magnetic */
	Bmx055Write8(BMX055_CHANNEL_MAG, 0x4B, 0x01);	// suspend -> sleep
	DelayMs(10 );

	Bmx055Write8(BMX055_CHANNEL_MAG, 0x4C, (0x00 << 3) | (0x00 << 1));	// sleep -> normal
	DelayMs(1);

	Bmx055Write8(BMX055_CHANNEL_MAG, 0x51, 7);	// REP XY : 2 * 7 + 1 = 15
	DelayMs(1);

	Bmx055Write8(BMX055_CHANNEL_MAG, 0x52, 26);	// REP Z : 26 + 1 = 27
	DelayMs(1);
}

void Bmx055GetAcc(uint8_t *buf, uint32_t n)
{
	Bmx055Read(BMX055_CHANNEL_ACC, 0x3F, buf, MIN(n, 6));
}

void Bmx055GetGyro(uint8_t *buf, uint32_t n)
{
	Bmx055Read(BMX055_CHANNEL_GYRO, 0x3F, buf, MIN(n, 6));
}

void Bmx055GetMag(uint8_t *buf, uint32_t n)
{
	Bmx055Read(BMX055_CHANNEL_MAG, 0x42, buf, MIN(n, 6));
}
